from maix import camera, display, image, nn, app , uart ,time
import struct

detector = nn.YOLOv5(model="/root/models/rect/model_3356.mud", dual_buff = True)


cam = camera.Camera(detector.input_width(), detector.input_height(), detector.input_format())
disp = display.Display()
device1 = "/dev/ttyS0"          # 示例的串口名称                        此串口设备名称为串口0
uart1 = uart.UART(device1 , 115200)

"""
    根据矩形的四个角点坐标计算中心点
    参数:
        points: 包含四个元组的列表，每个元组表示一个角点的(x, y)坐标，顺序不固定
    返回:
        中心点坐标(center_x, center_y)
    """
def calculate_rectangle_center(points):
    
    if len(points) != 4:
        print("需要提供4个角点坐标")
        return 0 ,0
    
    # 提取所有x坐标和y坐标
    x_coords = [p[0] for p in points]
    y_coords = [p[1] for p in points]
    
    # 计算中心点坐标
    center_x = sum(x_coords) / 4
    center_y = sum(y_coords) / 4
    return center_x, center_y

def uart_send(serial:uart.UART,head,data):
    send_data = struct.pack(f"<B{len(data)}h",head,*data)
    serial.write(send_data)



first_correct = -3


while not app.need_exit():
    img = cam.read()
    ex , ey = 5000 , 5000
    center_x = cam.width() / 2 + first_correct
    center_y = cam.height()/ 2
    objs = detector.detect(img, conf_th = 0.5, iou_th = 0.45)
    if objs:
        obj = max(objs , key = lambda obj:obj.score)
        area = obj.w * obj.h
        if area > 1000:
            img.draw_rect(obj.x, obj.y, obj.w, obj.h, color = image.COLOR_RED)
            msg = f'{detector.labels[obj.class_id]}: {obj.score:.2f}'
            img.draw_string(obj.x, obj.y, msg, color = image.COLOR_RED)
            c_x = obj.x + obj.w/2
            c_y = obj.y + obj.h/2
            c_x = int(c_x)
            c_y = int(c_y)
            ex = c_x - center_x
            ey = c_y - center_y
            img.draw_keypoints([c_x,c_y],color=image.COLOR_RED)


    uart_send(uart1,0xaa,[int(ex),-int(ey)])
    img.draw_keypoints([int(center_x),int(center_y)],color=image.COLOR_BLUE)
    img.draw_string(0,0,f"fps is {time.fps()}",color=image.COLOR_BLUE)
    img.draw_string(0,40,f"ex is {ex}",color=image.COLOR_BLUE)
    img.draw_string(0,80,f"ey is {-ey}",color=image.COLOR_BLUE)
        
    disp.show(img)
